using IQIGame.Onigao.Game;
using IQIGame.Onigao.GamePlay;
using RecastUnity;
using System;
using System.Collections.Generic;

namespace IQIGame.Onigao.Logic
{
    /// <summary>
    /// 关卡地形（寻路，实体四叉树，BVH树查询相关）
    /// </summary>
    public class LevelRegionTerrain : IDisposable
    {
        private const int DefaultHalfHeight = 400;
        private const int MaxObstacleRequestNum = 64;
        public float[] halfExtents { get; } = new float[3] { 0.5f, (float)DefaultHalfHeight / LgLevelConst.DistanceUnitRatio, 0.5f }; //米
        public RectLogic bounds2D => _bounds2D;
        public float heightRange => _heightRange;

        public IntPtr NavMeshScene { get; private set; }

        protected float[] sharedStartPos = new float[3];
        protected float[] sharedEndPos = new float[3];
        protected float[] sharedRealEndPos = new float[3];
        protected float[] smoothPath = new float[256 * 3];
        protected float[] sharedCenter = new float[3];
        protected float[] sharedHalfExtents = new float[3];

        private RectLogic _bounds2D;
        private float _heightRange;
        private FixedQuadTree<LevelEntity> _lgEntityQuadTree;
        private FixedBVHTree2D<LevelEntity> _lgEntityBVHTree;
        private int _regionCid;
        /// <summary>
        /// 当前添加或删除静态阻挡的请求个数（底层不论添加还是移除一次最多只支持64个）
        /// </summary>
        private int _addOrRemoveObstacleReqNum = 0;

        public LevelRegionTerrain(int regionCid)
        {
            _regionCid = regionCid;
        }

        public void Init()
        {
            float[] bmin = new float[3];
            float[] bmax = new float[3];
            _bounds2D = default;
            _heightRange = 0;
            NavMeshScene = LogicHelper.InitNavMesh(_regionCid);
            if (NavMeshScene == IntPtr.Zero)
            {
                return;
            }
            var result = RecastDll.RecastGetBounds(NavMeshScene, bmin, bmax);
            if (result != 0)
            {
                return;
            }

            Vector2Logic rectMin = new Vector2Logic(-bmax[0], bmin[2]) * LgLevelConst.DistanceUnitRatio;
            Vector2Logic rectMax = new Vector2Logic(-bmin[0], bmax[2]) * LgLevelConst.DistanceUnitRatio;
            Vector2Logic rectSize = rectMax - rectMin;
            _heightRange = (bmax[1] - bmin[1]) * LgLevelConst.DistanceUnitRatio;
            _bounds2D = new RectLogic(rectMin, rectSize);

            _lgEntityQuadTree = new FixedQuadTree<LevelEntity>(_bounds2D);
            _lgEntityBVHTree = new FixedBVHTree2D<LevelEntity>(FixedBVHBounds2D.FromRect(_bounds2D));
        }

        public bool FindNearestPoint(Vector3Logic startPos, out Vector3Logic realEndPos, int searchHalfHeight = DefaultHalfHeight)
        {
            if (NavMeshScene == IntPtr.Zero)
            {
                realEndPos = Vector3Logic.zero;
                return false;
            }

            //先刷新障碍物
            ForceUpdateObstacles();

            LgRecastUtility.PosInCM2RecastPos(startPos, sharedStartPos);

            int oldHeight = (int)halfExtents[1];
            halfExtents[1] = (float)searchHalfHeight / LgLevelConst.DistanceUnitRatio;
            int result = RecastDll.RecastFindNearestPoint(NavMeshScene, halfExtents, sharedStartPos, sharedRealEndPos);
            halfExtents[1] = oldHeight;

            if (result < 0)
            {
                realEndPos = Vector3Logic.zero;
                LogicLog.LogError($"Recast FindNearestPoint failed, result: {result}, targetPos: {startPos}");
                return false;
            }
            realEndPos = LgRecastUtility.RecastPos2PosInCM(sharedRealEndPos);
            return true;
        }

        public bool FindRandomPointAroundCircle(Vector3Logic centerPos, int radius, out Vector3Logic realEndPos)
        {
            if (NavMeshScene == IntPtr.Zero)
            {
                realEndPos = Vector3Logic.zero;
                return false;
            }

            //先刷新障碍物
            ForceUpdateObstacles();

            float meterRadius = LgLevelConvertUtil.LevelDistanceToMeter(radius);
            LgRecastUtility.PosInCM2RecastPos(centerPos, sharedStartPos);
            int result = RecastDll.RecastFindRandomPointAroundCircle(NavMeshScene, halfExtents, sharedStartPos, (float)meterRadius, sharedRealEndPos);

            if (result < 0)
            {
                realEndPos = Vector3Logic.zero;
                LogicLog.LogError($"Recast FindRandomPointAroundCircle failed, result: {result}, centerPos: {centerPos}, radius: {radius}");
                return false;
            }
            realEndPos = LgRecastUtility.RecastPos2PosInCM(sharedRealEndPos);
            return true;
        }

        public bool TryMove(Vector3Logic startPos, Vector3Logic endPos, out Vector3Logic realEndPos)
        {
            if (NavMeshScene == IntPtr.Zero)
            {
                realEndPos = Vector3Logic.zero;
                return false;
            }

            //先刷新障碍物
            ForceUpdateObstacles();

            LgRecastUtility.PosInCM2RecastPos(startPos, sharedStartPos);
            LgRecastUtility.PosInCM2RecastPos(endPos, sharedEndPos);

            int result = RecastDll.RecastTryMove(NavMeshScene, halfExtents, sharedStartPos, sharedEndPos, sharedRealEndPos);
            realEndPos = LgRecastUtility.RecastPos2PosInCM(sharedRealEndPos);

            return result >= 0;
        }

        public int FindPath(Vector3Logic startPos, Vector3Logic targetPos, out float[] path)
        {
            if (NavMeshScene == IntPtr.Zero)
            {
                path = null;
                return -1;
            }

            //先刷新障碍物
            ForceUpdateObstacles();

            LgRecastUtility.PosInCM2RecastPos(startPos, sharedStartPos);
            LgRecastUtility.PosInCM2RecastPos(targetPos, sharedEndPos);

            int smoothPathCount = RecastDll.RecastFindFollow(NavMeshScene, halfExtents, sharedStartPos, sharedEndPos, smoothPath);
            path = smoothPath;
            return smoothPathCount;
        }

        public uint AddObstacle(Vector3Logic position, Vector3Logic dir, ObstacleInfo info, int agentSpeed)
        {
            if (info == null || info.invalid || info.isClient)
            {
                return 0;
            }
            if (info.isStatic)
            {
                if (_addOrRemoveObstacleReqNum >= MaxObstacleRequestNum)
                {
                    ForceUpdateObstacles();
                    _addOrRemoveObstacleReqNum = 0;
                }
                uint obstacleId;
                if (info.isCapsule)
                {
                    obstacleId = AddCapsuleObstacle(position, info);
                }
                else
                {
                    obstacleId = AddRectObstacle(position, dir, info);
                }
                _addOrRemoveObstacleReqNum++;
                return obstacleId;
            }
            else
            {
                return (uint)AddAgent(position, info, agentSpeed);
            }
        }

        private uint AddRectObstacle(Vector3Logic position, Vector3Logic dir, ObstacleInfo info)
        {
            if (NavMeshScene == IntPtr.Zero)
            {
                return 0;
            }
            uint obstacleId = 0;
            Vector3Logic euler = Vector3Logic.DirectionToEulerXy(dir);
            LgRecastUtility.ObstacleInfo2RecastBox(position, info, sharedCenter, sharedHalfExtents, euler.y);
            int result = RecastDll.RecastAddBoxCenterObstacle(NavMeshScene, ref obstacleId, sharedCenter, sharedHalfExtents, euler.y, out uint errCode);
            if (result < 0)
            {
                LogicLog.LogError($"添加矩形阻挡失败。result: {result}, errCode: {errCode}, position: {position}, ObstacleInfo: {info}, yRotation: {euler.y}。");
                return 0;
            }
            return obstacleId;
        }

        private uint AddCapsuleObstacle(Vector3Logic position, ObstacleInfo info)
        {
            if (NavMeshScene == IntPtr.Zero)
            {
                return 0;
            }
            uint obstacleId = 0;
            LgRecastUtility.PosInCM2RecastPos(position, sharedStartPos);
            int result = RecastDll.RecastAddObstacle(NavMeshScene, ref obstacleId, sharedStartPos, (float)info.radius / LgLevelConst.DistanceUnitRatio, (float)info.height / LgLevelConst.DistanceUnitRatio, out uint errCode);
            if (result < 0)
            {
                LogicLog.LogError($"添加胶囊阻挡失败。result: {result}, errCode: {errCode}, position: {position}, ObstacleInfo: {info}。");
                return 0;
            }
            return obstacleId;
        }

        public int AddAgent(Vector3Logic position, ObstacleInfo info, int speed)
        {
            if (NavMeshScene == IntPtr.Zero)
            {
                return 0;
            }

            LgRecastUtility.PosInCM2RecastPos(position, sharedStartPos);
            int agentId = RecastDll.RecastAddAgent(NavMeshScene, sharedStartPos, (float)info.radius / LgLevelConst.DistanceUnitRatio, (float)info.height / LgLevelConst.DistanceUnitRatio, (float)speed / LgLevelConst.DistanceUnitRatio, (float)LgLevelConst.AgentMaxAcceleration / LgLevelConst.DistanceUnitRatio);
            if (agentId < 0)
            {
                LogicLog.LogError($"添加避障代理失败。agentId: {agentId}, position: {position}, ObstacleInfo: {info}, speed: {speed}。");
                return 0;
            }
            //RecastDll的问题，agentId==0有效，而且是底层返回的，但到entity层，不管是agentId还是ObstacleId都算ObstacleId，为了保证合法性，这里要+1转换
            return agentId + 1;
        }

        public void RemoveObstacle(bool isStatic, uint obstacleId)
        {
            if (NavMeshScene == IntPtr.Zero)
            {
                return;
            }

            if (obstacleId == 0)
            {
                return;
            }
            if (isStatic)
            {
                int result = RecastDll.RecastRemoveObstacle(NavMeshScene, obstacleId, out uint errCode);
                if (result < 0)
                {
                    LogicLog.LogError($"移除阻挡失败。result: {result}, errCode: {errCode}, obstacleId: {obstacleId}。");
                }
            }
            else
            {
                int agentId = ObstacleIdToAgentId(obstacleId);
                RecastDll.RecastRemoveAgent(NavMeshScene, agentId);
            }
        }

        /// <summary>
        /// 让agent寻路到目标点（避障模式，根据添加Agent时设置的速度需要一定的时间）
        /// 注意！一旦设置了这个接口，底层一定会将Agent寻路到目的地，即便使用SetAgentPos，寻路也不会停止，除非再次调用AgentHeadTo改变目的地
        /// </summary>
        /// <param name="obstacleId"></param>
        /// <param name="targetPos"></param>
        /// <returns></returns>
        public bool AgentHeadTo(uint obstacleId, Vector3Logic targetPos)
        {
            if (NavMeshScene == IntPtr.Zero)
            {
                return false;
            }
            int agentId = ObstacleIdToAgentId(obstacleId);
            LgRecastUtility.PosInCM2RecastPos(targetPos, sharedEndPos);
            int result = RecastDll.RecastSetAgentMoveTarget(NavMeshScene, agentId, sharedEndPos);
            if (result < 0)
            {
                LogicLog.LogError("Recast AgentHeadTo failed, result:" + result);
                return false;
            }
            return true;
        }

        /// <summary>
        /// 强制设置agent的位置，但需要注意，如果之前调用了AgentHeadTo()，再调用此方法，避障寻路并不会停止
        /// 也就是说，如果想要确切的马上将agent停止在指定位置，请调用TeleportAgentTo，或先调用AgentHeadTo(targetPos)，再调用此方法才行
        /// </summary>
        /// <param name="obstacleId"></param>
        /// <param name="targetPos"></param>
        /// <returns></returns>
        public bool SetAgentPos(uint obstacleId, Vector3Logic targetPos)
        {
            if (NavMeshScene == IntPtr.Zero)
            {
                return false;
            }
            //先刷新障碍物
            ForceUpdateObstacles();

            int agentId = ObstacleIdToAgentId(obstacleId);
            LgRecastUtility.PosInCM2RecastPos(targetPos, sharedEndPos);
            int result = RecastDll.RecastSetAgentPos(NavMeshScene, agentId, sharedEndPos);
            if (result < 0)
            {
                LogicLog.LogError("Recast SetAgentPos failed, result:" + result);
                return false;
            }
            return true;
        }

        /// <summary>
        /// 传送agent到指定位置，需要停止agent避障寻路，且目标点和当前点不相同，且需要马上生效时调用
        /// </summary>
        /// <param name="obstacleId"></param>
        /// <param name="targetPos"></param>
        /// <returns></returns>
        public bool TeleportAgentTo(uint obstacleId, Vector3Logic targetPos)
        {
            if (NavMeshScene == IntPtr.Zero)
            {
                return false;
            }
            int agentId = ObstacleIdToAgentId(obstacleId);
            LgRecastUtility.PosInCM2RecastPos(targetPos, sharedEndPos);
            //重设导航目标
            int result = RecastDll.RecastSetAgentMoveTarget(NavMeshScene, agentId, sharedEndPos);
            if (result < 0)
            {
                LogicLog.LogError("TeleportAgentTo error. RecastSetAgentMoveTarget failed, result:" + result);
                return false;
            }
            //先刷新障碍物
            ForceUpdateObstacles();
            //直接设置agent位置
            result = RecastDll.RecastSetAgentPos(NavMeshScene, agentId, sharedEndPos);
            if (result < 0)
            {
                LogicLog.LogError("TeleportAgentTo error. RecastSetAgentPos failed, result:" + result);
                return false;
            }
            return true;
        }

        public Vector3Logic GetAgentPos(uint obstacleId)
        {
            if (NavMeshScene == IntPtr.Zero)
            {
                return Vector3Logic.zero;
            }

            int agentId = ObstacleIdToAgentId(obstacleId);
            RecastDll.RecastGetAgentPos(NavMeshScene, agentId, sharedEndPos);
            return LgRecastUtility.RecastPos2PosInCM(sharedEndPos);
        }

        public bool ChangeAgentSpeed(uint obstacleId, int speed, int acceleration)
        {
            if (NavMeshScene == IntPtr.Zero)
            {
                return false;
            }

            int agentId = ObstacleIdToAgentId(obstacleId);
            int result = RecastDll.RecastSetAgentSpeed(NavMeshScene, agentId, (float)speed / LgLevelConst.DistanceUnitRatio, (float)acceleration / LgLevelConst.DistanceUnitRatio);
            if (result < 0)
            {
                LogicLog.LogError("Recast ChangeAgentSpeed failed, result:" + result);
                return false;
            }
            return true;
        }

        public void LogicOnUpdate()
        {
            RecastDll.RecastUpdate(NavMeshScene, (float)LgLevelConst.deltaTime);
            //每帧单个地刷新缓存里add，remove obstacle的数据，性能更平稳
            RecastDll.RecastUpdateObstacles(NavMeshScene, true);
            //由于每帧单个刷新obstacle，所以这里的请求容量数量--
            if (_addOrRemoveObstacleReqNum > 0)
            {
                _addOrRemoveObstacleReqNum--;
            }
        }

        public void ForceUpdateObstacles()
        {
            if (NavMeshScene == IntPtr.Zero)
            {
                return;
            }

            //一帧直接把缓存里待刷新的add，remove obstacle的数据刷完，性能不如isEveryFrame == true平稳
            //如果缓存里无数据，则几乎无消耗
            RecastDll.RecastUpdateObstacles(NavMeshScene, false);
        }

        private int ObstacleIdToAgentId(uint obstacleId)
        {
            //RecastDll的问题，agentId==0有效，而且是底层返回的，但到entity层，不管是agentId还是ObstacleId都算ObstacleId，
            //为了保证合法性，ObstacleId = agentId + 1
            return (int)obstacleId - 1;
        }

        #region quad tree api
        public void AddIntoQuadTree(LevelEntity entity)
        {
            entity.quadTreePos = entity.GetPosition2D();
            _lgEntityQuadTree.Insert(entity);
        }

        public void RemoveFromQuadTree(LevelEntity entity)
        {
            _lgEntityQuadTree.Remove(entity);
        }

        public void MoveInQuadTree(LevelEntity entity)
        {
            _lgEntityQuadTree.Update(entity, entity.GetPosition2D());
        }

        public void QuadSearchEntity(List<LevelEntity> queryList, Vector3Logic heroPos)
        {
            float halfSize = LgLevelConst.HeroQuadQueryHalfSize;
            float minX = heroPos.x - halfSize;
            float minZ = heroPos.z - halfSize;
            float fullSize = halfSize * 2;
            RectLogic area = new RectLogic(minX, minZ, fullSize, fullSize);
            _lgEntityQuadTree.RetrieveObjects(queryList, area);
        }

        public void QuadSearchEntity(List<LevelEntity> queryList, RectLogic searchArea)
        {
            _lgEntityQuadTree.RetrieveObjects(queryList, searchArea);
        }
        #endregion

        #region bvh tree api
        public void AddIntoBvhTree(LevelEntity entity)
        {
            entity.bvhBounds = entity.GetBvhBounds();
            _lgEntityBVHTree.Insert(entity);
        }

        public void RemoveFromBvhTree(LevelEntity entity)
        {
            _lgEntityBVHTree.Remove(entity);
        }

        public void MoveInBvhTree(LevelEntity entity)
        {
            _lgEntityBVHTree.Update(entity, entity.GetBvhBounds());
        }

        public void BvhSearchEntity(List<LevelEntity> queryList, Vector3Logic position, Vector3Logic size, int rotationY)
        {
            FixedBVHBounds2D bvhBounds;
            if (rotationY != 0)
            {
                bvhBounds = FixedBVHBounds2D.FromPosSizeRotationY(position, size, rotationY);
            }
            else
            {
                bvhBounds = FixedBVHBounds2D.FromPosSize(position, size);
            }
            _lgEntityBVHTree.RetrieveObjects(queryList, bvhBounds);
        }
        #endregion

        public bool CheckInTerrainBounds(Vector3Logic position)
        {
            return _bounds2D.Contains(new Vector2Logic(position.x, position.z));
        }

        public void Dispose()
        {
            LogicHelper.CleanNavMeshScenePtr(NavMeshScene);
            NavMeshScene = IntPtr.Zero;
        }
    }
}
